//
// Created by cybaster on 25-5-27.
//

#include "ocs2_humanoid_robot_controller/controller/HumanoidRobotWBCController.h"

#include <legged_wbc/WeightedWbc.h>

int main(int argc, char **argv) {
  const std::string robotName = "legged_robot";

  ros::init(argc, argv, robotName + "_mpc_controller");
  ros::NodeHandle nodeHandle;

  std::string taskFile, urdfFile, referenceFile;
  nodeHandle.getParam("/taskFile", taskFile);
  nodeHandle.getParam("/referenceFile", referenceFile);
  nodeHandle.getParam("/urdfFile", urdfFile);

  LeggedRobotInterface interface(taskFile, urdfFile, referenceFile);

  CentroidalModelPinocchioMapping pinocchioMapping(
      interface.getCentroidalModelInfo());
  auto endEffectorKinematicsPtr =
      std::make_shared<PinocchioEndEffectorKinematics>(
          interface.getPinocchioInterface(), pinocchioMapping,
          interface.modelSettings().contactNames3DoF);
  auto wbcController = std::make_shared<legged::WeightedWbc>(
      interface.getPinocchioInterface(), interface.getCentroidalModelInfo(),
      *endEffectorKinematicsPtr);
  bool verbose = false;
  wbcController->loadTasksSetting(taskFile, verbose);

  HumanoidRobotWBCController controller(interface);
  controller.setWBCController(wbcController);

  controller.launchNodes(nodeHandle);

  return 0;
}